#include "auto_aim.h"
#include "remote.h"
#include "vision.h"
#include "arm_math.h"
#include "imu.h"
#include <math.h>

/* 内部自定义数据类型的变量 --------------------------------------------------*/

#define Z_BC 						0
#define right_angle_r				8*M_PI/18



#define INFANTRY 					-1
#define Z_Outpost_BC				10


float Z_compensate;
float Z_compensate_input;

uint8_t rotate_flag;
uint8_t fire_status;
uint8_t wait_armor, track_armor;
uint8_t armor_status, track_status;

extern int8_t rotate_direction;

//反解pitch轴角度变量
target_spac_t* determine_target;
target_spac_t target_space_aim;
//弹道解算变量
ballistic_sol_t ballistic_solusion; 

//反陀螺测试用全局变量，测试完成后替换成局部变量
float watch_111, watch_222, watch_333, watch_444, watch_555,watch_666;

//装甲板状态更新函数
void armor_update(void);
void projectile_solve(float v0, target_spac_t* target, ballistic_sol_t* solution);
//陀螺旋转方向判断函数


uint8_t autoaim_fire_control(float yaw_fire_angle_min, float yaw_fire_angle_max, float pitch_fire_angle_min, float pitch_fire_angle_max);
uint8_t aimcentre_fire_control(float yaw_armor_left, float yaw_armor_right, float pitch_fire_angle_min, float pitch_fire_angle_max);

//===================================================================================================================//
/****************************************************Ballistic_mode**************************************************/
//===================================================================================================================//


uint8_t Woody_Autoaim(uint8_t shoot_speed)
{
	//识别到的装甲板ID
	uint8_t armor_id;
	//时间的单位为s
	float pre_time;
	float bullet_fly_time;
	float shoot_delay_time;
	float vision_delay_time;
	//PC原始数据
	float R_armor;
	float orientation_angle_r;
	float X_speed,	Y_speed,	Z_speed;
	float X_centre, Y_centre, Z_centre;
	//后处理数据
	float X_armor, Y_armor, Z_armor;
	float Z_compensate, Z_compensate_pre;
	float X_centre_pre, Y_centre_pre, Z_centre_pre;
	//需要开根号计算的长度变量，单位为mm
	float target_distance, target_distance_input;
	float centre_distance, centre_distance_input;
	float centre_distance_pre, centre_distance_pre_input;
	float bullet_fly_distance, bullet_fly_distance_input;
	//目标装甲板参数
	float target_armor_width;
	float target_armor_height;
	//目标装甲板边界坐标
	float Z_target_armor_up, Z_target_armor_down;
	float X_target_armor_left, Y_target_armor_left;
	float X_target_armor_right, Y_target_armor_right;
	//火控允许开火角度范围
	static float yaw_allow_fire_angle_min_r, yaw_allow_fire_angle_max_r;
	static float pitch_allow_fire_angle_min_r, pitch_allow_fire_angle_max_r;
	//云台火控瞄准角度
	float yaw_target_armor, yaw_target_armor_left, yaw_target_armor_right;
	float pitch_target_armor, pitch_target_armor_up, pitch_target_armor_down;
	//最终目标变量
	float X_target_armor, Y_target_armor, Z_target_armor;
	//弹道解算存储结构体
	target_spac_t  target_armor_aim;
	target_spac_t  target_armor_up_aim;
	target_spac_t  target_armor_down_aim;
	//弹道解算反解pitch轴结构体
	ballistic_sol_t ballistic_solusion; 
	ballistic_sol_t ballistic_solusion_up;
	ballistic_sol_t ballistic_solusion_down;
	//判断视觉是否丢失数据
	if(minipc_rx.state_flag == 0)
	{
		target_set.yaw_targetangle_gyro		= INS.Yaw.total_radangle;
		target_set.pitch_targetangle_gyro	= INS.Pitch.total_radangle;
		//视觉识别错误
		return Vision_Error;
	}
	//读取PC数据
	X_speed 						= minipc_rx.X_speed;
	Y_speed 					   	= minipc_rx.Y_speed;
	Z_speed 						= minipc_rx.Z_speed;
	X_centre 						= minipc_rx.X_coordinate;
	Y_centre 					   	= minipc_rx.Y_coordinate;
	Z_centre 						= minipc_rx.Z_coordinate;
	R_armor							= minipc_rx.radius_1;
	armor_id					   	= minipc_rx.armor_id;
	vision_delay_time 	 = minipc_rx.process_time / 100000.0f;
	orientation_angle_r  = minipc_rx.Yaw_coordinate;
	//计算敌方车辆中心距离，单位为mm
	centre_distance_input = X_centre*X_centre + Y_centre*Y_centre;
	arm_sqrt_f32(centre_distance_input, &centre_distance);
	//计算z轴补偿高度，线性补偿，补偿单位为mm
	Z_compensate	= Z_BC * (centre_distance / 1000.0f);
	//计算装甲板的现在坐标，单位为mm
	X_armor = X_centre - R_armor*arm_cos_f32(orientation_angle_r);
	Y_armor = Y_centre - R_armor*arm_sin_f32(orientation_angle_r);
	Z_armor = Z_centre + Z_compensate;
	//计算弹丸的飞行时间，单位为s
	bullet_fly_distance_input = X_armor*X_armor + Y_armor*Y_armor + Z_armor*Z_armor;
	arm_sqrt_f32(bullet_fly_distance_input, &bullet_fly_distance);
	bullet_fly_time = (bullet_fly_distance / 1000.0f) / shoot_speed;
	//调试发射延时
	shoot_delay_time = 0.02f;
	//计算预测时间
	pre_time = bullet_fly_time + shoot_delay_time + vision_delay_time;
	//预测位置信息
	X_centre_pre = X_centre + X_speed*pre_time;
	Y_centre_pre = Y_centre + Y_speed*pre_time;
	Z_centre_pre = Z_centre + Z_speed*pre_time;
	//计算敌方车辆预测中心距离，单位为mm
	centre_distance_pre_input = X_centre_pre*X_centre_pre + Y_centre_pre*Y_centre_pre;
	arm_sqrt_f32(centre_distance_pre_input, &centre_distance_pre);
	//计算z轴预测补偿高度，线性补偿y=kx，补偿单位为mm
	Z_compensate_pre	= Z_BC * (centre_distance_pre / 1000.0f);
	//计算装甲板的预测坐标，单位为mm
	X_target_armor = X_centre_pre - R_armor*arm_cos_f32(orientation_angle_r);
	Y_target_armor = Y_centre_pre - R_armor*arm_sin_f32(orientation_angle_r);
	Z_target_armor = Z_centre_pre + Z_compensate_pre;
	//计算跟踪装甲板yaw轴期望值，弧度单位
	yaw_target_armor = atan2(Y_target_armor, X_target_armor);
	//计算目标装甲板对应的距离，单位为mm
	target_distance_input = X_target_armor*X_target_armor + Y_target_armor*Y_target_armor;
	arm_sqrt_f32(target_distance_input, &target_distance);
	//目标装甲板长度变量赋值，单位：m
	target_armor_aim.xy0 	= target_distance / 1000.0f;
	target_armor_aim.z0		= Z_target_armor / 1000.0f;
	//计算跟踪装甲板pitch轴期望值，弧度单位
	projectile_solve(shoot_speed, &target_armor_aim, &ballistic_solusion);
	pitch_target_armor = INFANTRY*ballistic_solusion.ang_solution1;
	//判断装甲板宽度
	if(armor_id == 1)
	{
		target_armor_width  = large_armor_width;
		target_armor_height = large_armor_height;
	}
	else
	{
		target_armor_width  = small_armor_width;
		target_armor_height = small_armor_height;
	}
	//计算装甲板边界坐标
	Z_target_armor_up		 = Z_target_armor + 0.5f * target_armor_height*arm_cos_f32(armor_slant_angle);
	Z_target_armor_down	 = Z_target_armor - 0.5f * target_armor_height*arm_cos_f32(armor_slant_angle);
	X_target_armor_left  = X_target_armor - 0.5f * target_armor_width*arm_sin_f32(orientation_angle_r);
	Y_target_armor_left  = Y_target_armor + 0.5f * target_armor_width*arm_cos_f32(orientation_angle_r);
	X_target_armor_right = X_target_armor + 0.5f * target_armor_width*arm_sin_f32(orientation_angle_r);
	Y_target_armor_right = Y_target_armor - 0.5f * target_armor_width*arm_cos_f32(orientation_angle_r);
	//计算俯仰角边界角度
	target_armor_up_aim.xy0 	= target_distance / 1000.0f;
	target_armor_up_aim.z0		= Z_target_armor_up / 1000.0f;
	target_armor_down_aim.xy0 = target_distance / 1000.0f;
	target_armor_down_aim.z0	= Z_target_armor_down / 1000.0f;
	projectile_solve(shoot_speed, &target_armor_up_aim, &ballistic_solusion_up);
	projectile_solve(shoot_speed, &target_armor_down_aim, &ballistic_solusion_down);
	//计算目标装甲板火控角度
	pitch_target_armor_up 	= INFANTRY*ballistic_solusion_up.ang_solution1;
	pitch_target_armor_down = INFANTRY*ballistic_solusion_down.ang_solution1;
	yaw_target_armor_left 	= atan2(Y_target_armor_left, X_target_armor_left);
	yaw_target_armor_right 	= atan2(Y_target_armor_right, X_target_armor_right);
	//计算允许开火角度范围
	yaw_allow_fire_angle_min_r = yaw_target_armor - yaw_target_armor_left;
	yaw_allow_fire_angle_max_r = yaw_target_armor - yaw_target_armor_right;
	pitch_allow_fire_angle_min_r = pitch_target_armor - pitch_target_armor_down;
	pitch_allow_fire_angle_max_r = pitch_target_armor - pitch_target_armor_up;
	//计算跟踪装甲板期望值，弧度单位
	target_set.yaw_targetangle_gyro 	= yaw_target_armor + INS.Yaw.angle_round*2*M_PI;
	target_set.pitch_targetangle_gyro = pitch_target_armor;
	//控制yaw轴期望值范围
	while(target_set.yaw_targetangle_gyro - INS.Yaw.total_radangle > M_PI)target_set.yaw_targetangle_gyro -= 2*M_PI;
	while(target_set.yaw_targetangle_gyro - INS.Yaw.total_radangle < -M_PI)target_set.yaw_targetangle_gyro += 2*M_PI;
	//判断发射状态
	fire_status = autoaim_fire_control(yaw_allow_fire_angle_min_r, yaw_allow_fire_angle_max_r, pitch_allow_fire_angle_min_r, pitch_allow_fire_angle_max_r);
	//视觉识别正常
	return Vision_OK;

}

uint8_t Woody_Antitop(uint8_t shoot_speed)
{
	//识别到的装甲板ID
	uint8_t armor_id;
	//转速量纲为rad/s
	float rotate_speed_rps;
	float yaw_motor_res_speed_rps;
	//时间量纲为s
	float pre_time;
	float bullet_fly_time;
	float shoot_delay_time;
	float vision_delay_time;
	float yaw_motor_res_time;
	//角度量纲为rad
	float advanced_angle_r;
	float yaw_angle_diff_r;
	float centre_angle_pre_r;
	float orientation_angle_r;
	//PC原始数据
	float Z_armor_err;
	float radius_1, radius_2;
	float X_speed,	Y_speed,	Z_speed;
	float X_centre, Y_centre, Z_centre;
	//后处理坐标数据，量纲为mm
	float X_armor, Y_armor, Z_armor;
	float Z_compensate, Z_compensate_pre;
	float X_centre_pre, Y_centre_pre, Z_centre_pre;
	//所有装甲板数据，量纲为mm
	float R_armor[4];
	float orientation_angle_pre_r[4];
	float abs_orientation_angle_pre_r[4];
	float X_armor_pre[4], Y_armor_pre[4], Z_armor_pre[4];
	//需要开根号计算的长度变量，量纲为mm
	float target_distance, target_distance_input;
	float centre_distance, centre_distance_input;
	float centre_distance_pre, centre_distance_pre_input;
	float bullet_fly_distance, bullet_fly_distance_input;
	//目标装甲板参数
	float target_armor_width;
	float target_armor_height;
	//目标装甲板边界坐标
	float Z_target_armor_up, Z_target_armor_down;
	float X_target_armor_left, Y_target_armor_left;
	float X_target_armor_right, Y_target_armor_right;
	//火控允许开火角度范围
	float yaw_allow_fire_angle_min_r, yaw_allow_fire_angle_max_r;
	float pitch_allow_fire_angle_min_r, pitch_allow_fire_angle_max_r;
	//云台火控瞄准角度
	float yaw_target_armor, yaw_target_armor_left, yaw_target_armor_right;
	float pitch_target_armor, pitch_target_armor_up, pitch_target_armor_down;
	//目标装甲板相关变量

	//弹道解算存储结构体
	target_spac_t  target_armor_aim;
	target_spac_t  target_armor_up_aim;
	target_spac_t  target_armor_down_aim;
	//弹道解算反解pitch轴结构体
	ballistic_sol_t ballistic_solusion; 
	ballistic_sol_t ballistic_solusion_up;
	ballistic_sol_t ballistic_solusion_down;
	//判断视觉是否丢失数据
	if(minipc_rx.state_flag == 0)
	{
		target_set.yaw_targetangle_gyro		= INS.Yaw.total_radangle;
		target_set.pitch_targetangle_gyro	= INS.Pitch.total_radangle;
		return Vision_Error;
	}
	//读取PC数据
	X_speed 						= minipc_rx.X_speed;
	Y_speed 						= minipc_rx.Y_speed;
	Z_speed 						= minipc_rx.Z_speed;
	X_centre 						= minipc_rx.X_coordinate;
	Y_centre 						= minipc_rx.Y_coordinate;
	Z_centre 						= minipc_rx.Z_coordinate;
	Z_armor_err 					= minipc_rx.DZ;
	armor_id						= minipc_rx.armor_id;
	radius_1						= minipc_rx.radius_1;
	radius_2						= minipc_rx.radius_2;
	orientation_angle_r	= minipc_rx.Yaw_coordinate;

	#ifdef GIMBAL_BOARD_DOWN
	rotate_speed_rps 		= minipc_rx.Yaw_speed * 1.2f;
	#endif
	#ifdef GIMBAL_BOARD_UP
	rotate_speed_rps 		= minipc_rx.Yaw_speed * 1.25f;
	#endif
	vision_delay_time 	= minipc_rx.process_time / 100000.0f;
	//判断装甲板尺寸
	if(armor_id == 1)
	{
		target_armor_width  = large_armor_width;
		target_armor_height = large_armor_height;
	}
	else
	{
		target_armor_width  = small_armor_width;
		target_armor_height = small_armor_height;
	}
	//装甲板识别更新
	armor_update();
	//调试电机旋转速度
	yaw_motor_res_speed_rps = 1.0f;
	//计算敌方车辆中心距离，单位为mm
	centre_distance_input = X_centre*X_centre + Y_centre*Y_centre;
	arm_sqrt_f32(centre_distance_input, &centre_distance);
	//计算z轴补偿高度，线性补偿y=kx，补偿单位为mm
	Z_compensate	= Z_BC * (centre_distance / 1000.0f);
	//计算识别装甲板坐标
	X_armor = X_centre - radius_1*arm_cos_f32(orientation_angle_r);
	Y_armor = Y_centre - radius_1*arm_sin_f32(orientation_angle_r);
	Z_armor = Z_centre + Z_compensate;
	//计算弹丸飞行距离
	bullet_fly_distance_input = X_armor*X_armor + Y_armor*Y_armor + Z_armor*Z_armor;
	arm_sqrt_f32(bullet_fly_distance_input, &bullet_fly_distance);
	//计算弹丸飞行时间
	bullet_fly_time = (bullet_fly_distance / 1000.0f) / shoot_speed;
	//调试发射延时
	shoot_delay_time = 0.0f;
	//计算预测时间
	pre_time = bullet_fly_time + shoot_delay_time + vision_delay_time;
	//预测位置信息
	X_centre_pre = X_centre + X_speed*pre_time;
	Y_centre_pre = Y_centre + Y_speed*pre_time;
	Z_centre_pre = Z_centre + Z_speed*pre_time;
	orientation_angle_pre_r[identify_armor] = orientation_angle_r + rotate_speed_rps*pre_time;
	//计算瞄中心云台偏转角
	centre_angle_pre_r = atan2(Y_centre_pre, X_centre_pre);
	//计算敌方车辆预测中心距离，单位为mm
	centre_distance_pre_input = X_centre_pre*X_centre_pre + Y_centre_pre*Y_centre_pre;
	arm_sqrt_f32(centre_distance_pre_input, &centre_distance_pre);
	//计算z轴预测补偿高度，线性补偿y=kx，补偿单位为mm
	Z_compensate_pre	= Z_BC * (centre_distance_pre / 1000.0f);
	//计算四块装甲板的数据
	for(uint8_t i = 0; i < 4; i++)
	{
		//分配装甲板半径与预测Z坐标
		if(i % 2 == 0)
		{
			R_armor[i] = radius_1;
			Z_armor_pre[i] = Z_centre_pre + Z_compensate_pre;
		}
		else
		{
			R_armor[i] = radius_2;
			Z_armor_pre[i] = Z_centre_pre + Z_compensate_pre + Z_armor_err;
		}
		//计算四块装甲板的预测朝向角与预测绝对筛选角，单位为rad
		orientation_angle_pre_r[i] = orientation_angle_pre_r[identify_armor] + i*right_angle_r*rotate_direction;
		abs_orientation_angle_pre_r[i] = orientation_angle_pre_r[i] - centre_angle_pre_r;
		//计算四块装甲板的预测坐标，单位为mm
		X_armor_pre[i] = X_centre_pre - R_armor[i]*arm_cos_f32(orientation_angle_pre_r[i]);
		Y_armor_pre[i] = Y_centre_pre - R_armor[i]*arm_sin_f32(orientation_angle_pre_r[i]);
	}

	switch(rotate_direction)
	{
		case AntiClockwise:
		{
			if(abs_orientation_angle_pre_r[track_armor] < M_PI/4)
			{
				track_status = 1;
				
				yaw_angle_diff_r 	= atan2(Y_armor_pre[track_armor], X_armor_pre[track_armor]) - atan2(Y_armor_pre[track_armor + 1], X_armor_pre[track_armor + 1]);
				yaw_motor_res_time 	= fabsf(yaw_angle_diff_r) / yaw_motor_res_speed_rps;
				advanced_angle_r 	= fabsf(rotate_speed_rps) * yaw_motor_res_time / 2.0f;
				
				if(abs_orientation_angle_pre_r[track_armor] < (M_PI/4 - advanced_angle_r))
				{	

				}
				else
				{
					track_armor++;
					
					if(track_armor > 3)
						track_armor = 3;
				}
			}
			else
			{
				track_status = 2;
			}
		}break;
		
		case Clockwise:
		{
			if(abs_orientation_angle_pre_r[track_armor] > -M_PI/4)
			{
				track_status = 1;
				
				yaw_angle_diff_r 		= atan2(Y_armor_pre[track_armor], X_armor_pre[track_armor]) - atan2(Y_armor_pre[track_armor + 1], X_armor_pre[track_armor + 1]);
				yaw_motor_res_time 	= fabsf(yaw_angle_diff_r) / yaw_motor_res_speed_rps;
				advanced_angle_r 		= fabsf(rotate_speed_rps) * yaw_motor_res_time / 2.0f;				
				
				if(abs_orientation_angle_pre_r[track_armor] > (-M_PI/4 + advanced_angle_r))
				{				

				}
				else
				{
					track_armor++;
					
					if(track_armor > 3)
						track_armor = 3;							

				}
			}
			else
			{
				track_status = 2;
			}
		}break;
		
		default: break;
	}



	//计算跟踪装甲板yaw轴期望值，弧度单位
	yaw_target_armor = atan2(Y_armor_pre[track_armor], X_armor_pre[track_armor]);
	//计算目标装甲板对应的距离，单位为mm
	target_distance_input = X_armor_pre[track_armor]*X_armor_pre[track_armor] + Y_armor_pre[track_armor]*Y_armor_pre[track_armor];
	arm_sqrt_f32(target_distance_input, &target_distance);
	//目标装甲板长度变量赋值，单位：m
	target_armor_aim.xy0 	= target_distance / 1000.0f;
	target_armor_aim.z0		= Z_armor_pre[track_armor] / 1000.0f;
	//计算跟踪装甲板pitch轴期望值，弧度单位
	projectile_solve(shoot_speed, &target_armor_aim, &ballistic_solusion);
	pitch_target_armor = INFANTRY*ballistic_solusion.ang_solution1;
	//计算装甲板边界坐标
	Z_target_armor_up	 = Z_armor_pre[track_armor] + 0.5f * target_armor_height*arm_cos_f32(armor_slant_angle);
	Z_target_armor_down	 = Z_armor_pre[track_armor] - 0.5f * target_armor_height*arm_cos_f32(armor_slant_angle);
	X_target_armor_left  = X_armor_pre[track_armor] - 0.5f * target_armor_width*arm_sin_f32(orientation_angle_pre_r[track_armor]);
	Y_target_armor_left  = Y_armor_pre[track_armor] + 0.5f * target_armor_width*arm_cos_f32(orientation_angle_pre_r[track_armor]);
	X_target_armor_right = X_armor_pre[track_armor] + 0.5f * target_armor_width*arm_sin_f32(orientation_angle_pre_r[track_armor]);
	Y_target_armor_right = Y_armor_pre[track_armor] - 0.5f * target_armor_width*arm_cos_f32(orientation_angle_pre_r[track_armor]);
	//计算俯仰角边界角度
	target_armor_up_aim.xy0 	= target_distance / 1000.0f;
	target_armor_up_aim.z0		= Z_target_armor_up / 1000.0f;
	target_armor_down_aim.xy0 = target_distance / 1000.0f;
	target_armor_down_aim.z0	= Z_target_armor_down / 1000.0f;
	projectile_solve(shoot_speed, &target_armor_up_aim, &ballistic_solusion_up);
	projectile_solve(shoot_speed, &target_armor_down_aim, &ballistic_solusion_down);
	//计算目标装甲板火控角度
	yaw_target_armor_left 	= atan2(Y_target_armor_left, X_target_armor_left);
	yaw_target_armor_right 	= atan2(Y_target_armor_right, X_target_armor_right);
	pitch_target_armor_up 	= INFANTRY*ballistic_solusion_up.ang_solution1;
	pitch_target_armor_down = INFANTRY*ballistic_solusion_down.ang_solution1;
	//计算允许开火角度范围
	yaw_allow_fire_angle_min_r = yaw_target_armor - yaw_target_armor_left;
	yaw_allow_fire_angle_max_r = yaw_target_armor - yaw_target_armor_right;
	pitch_allow_fire_angle_min_r = pitch_target_armor - pitch_target_armor_down;
	pitch_allow_fire_angle_max_r = pitch_target_armor - pitch_target_armor_up;
	//计算跟踪装甲板期望值，弧度单位
	target_set.yaw_targetangle_gyro = yaw_target_armor + INS.Yaw.angle_round*2*M_PI;
	target_set.pitch_targetangle_gyro = pitch_target_armor;
	//控制yaw轴期望值范围
	while(target_set.yaw_targetangle_gyro - INS.Yaw.total_radangle > M_PI)target_set.yaw_targetangle_gyro -= 2*M_PI;
	while(target_set.yaw_targetangle_gyro - INS.Yaw.total_radangle < -M_PI)target_set.yaw_targetangle_gyro += 2*M_PI;
	//判断发射状态
	fire_status = autoaim_fire_control(yaw_allow_fire_angle_min_r, yaw_allow_fire_angle_max_r, pitch_allow_fire_angle_min_r, pitch_allow_fire_angle_max_r);
	//视觉识别正常
	return Vision_OK;
} 

uint8_t Woody_Aimcentre(uint8_t shoot_speed)
{
	//识别到的装甲板ID
	uint8_t armor_id;
	//转速量纲为rad/s
	float rotate_speed_rps;
	//时间量纲为s
	float pre_time;
	float bullet_fly_time;
	float shoot_delay_time;
	float vision_delay_time;
	//角度量纲为rad
	float centre_angle_pre_r;
	float orientation_angle_r;
	//PC原始数据
	float Z_armor_err;
	float radius_1, radius_2;
	float X_speed,	Y_speed,	Z_speed;
	float X_centre, Y_centre, Z_centre;
	//后处理坐标数据，量纲为mm
	float X_armor, Y_armor, Z_armor;
	float Z_compensate, Z_compensate_pre;
	float X_centre_pre, Y_centre_pre, Z_centre_pre;
	//所有装甲板数据，量纲为mm
	float R_armor[4];
	float orientation_angle_pre_r[4];
	float abs_orientation_angle_pre_r[4];
	float X_armor_pre_left[4], Y_armor_pre_left[4];
	float X_armor_pre_right[4], Y_armor_pre_right[4];
	float X_armor_pre[4], Y_armor_pre[4], Z_armor_pre[4];
	//需要开根号计算的长度变量，量纲为mm
	float target_distance, target_distance_input;
	float centre_distance, centre_distance_input;
	float centre_distance_pre, centre_distance_pre_input;
	float bullet_fly_distance, bullet_fly_distance_input;
	//目标装甲板参数
	float target_armor_width;
	float target_armor_height;
	//目标装甲板边界坐标
	float Z_target_armor_up, Z_target_armor_down;
	//火控允许开火角度范围
	float pitch_allow_fire_angle_min_r, pitch_allow_fire_angle_max_r;
	//云台火控瞄准角度
	float yaw_armor_pre_left[4], yaw_armor_pre_right[4];
	float yaw_target_armor;
	float pitch_target_armor, pitch_target_armor_up, pitch_target_armor_down;
	//目标装甲板相关变量

	//弹道解算存储结构体
	target_spac_t  target_armor_aim;
	target_spac_t  target_armor_up_aim;
	target_spac_t  target_armor_down_aim;
	//弹道解算反解pitch轴结构体
	ballistic_sol_t ballistic_solusion; 
	ballistic_sol_t ballistic_solusion_up;
	ballistic_sol_t ballistic_solusion_down;
	//判断视觉是否丢失数据
	if(minipc_rx.state_flag == 0)
	{
		target_set.yaw_targetangle_gyro		= INS.Yaw.total_radangle;
		target_set.pitch_targetangle_gyro	= INS.Pitch.total_radangle;
		return Vision_Error;
	}
	//读取PC数据
	X_speed 						= minipc_rx.X_speed;
	Y_speed 						= minipc_rx.Y_speed;
	Z_speed 						= minipc_rx.Z_speed;
	X_centre 						= minipc_rx.X_coordinate;
	Y_centre 						= minipc_rx.Y_coordinate;
	Z_centre 						= minipc_rx.Z_coordinate;
	Z_armor_err 					= minipc_rx.DZ;
	armor_id						= minipc_rx.armor_id;
	radius_1						= minipc_rx.radius_1;
	radius_2						= minipc_rx.radius_2;
	orientation_angle_r	= minipc_rx.Yaw_coordinate;
	#ifdef GIMBAL_BOARD_DOWN
	rotate_speed_rps 		= minipc_rx.Yaw_speed * 1.2f;
	#endif
	#ifdef GIMBAL_BOARD_UP
	rotate_speed_rps 		= minipc_rx.Yaw_speed * 1.25f;
	#endif
	vision_delay_time 	= minipc_rx.process_time / 100000.0f;
	//判断装甲板尺寸
	if(armor_id == 1)
	{
		target_armor_width  = large_armor_width;
		target_armor_height = large_armor_height;
	}
	else
	{
		target_armor_width  = small_armor_width;
		target_armor_height = small_armor_height;
	}
	//装甲板识别更新
	armor_update();
	//计算敌方车辆中心距离，单位为mm
	centre_distance_input = X_centre*X_centre + Y_centre*Y_centre;
	arm_sqrt_f32(centre_distance_input, &centre_distance);
	//计算z轴补偿高度，线性补偿y=kx，补偿单位为mm
	Z_compensate	= Z_BC * (centre_distance / 1000.0f);
	//计算识别装甲板坐标
	X_armor = X_centre - radius_1*arm_cos_f32(orientation_angle_r);
	Y_armor = Y_centre - radius_1*arm_sin_f32(orientation_angle_r);
	Z_armor = Z_centre + Z_compensate;
	//计算弹丸飞行距离
	bullet_fly_distance_input = X_armor*X_armor + Y_armor*Y_armor + Z_armor*Z_armor;
	arm_sqrt_f32(bullet_fly_distance_input, &bullet_fly_distance);
	//计算弹丸飞行时间
	bullet_fly_time = (bullet_fly_distance / 1000.0f) / shoot_speed;
	//调试发射延时
	shoot_delay_time = 0.0f;
	//计算预测时间
	pre_time = bullet_fly_time + shoot_delay_time + vision_delay_time;
	//预测位置信息
	X_centre_pre = X_centre + X_speed*pre_time;
	Y_centre_pre = Y_centre + Y_speed*pre_time;
	Z_centre_pre = Z_centre + Z_speed*pre_time;
	orientation_angle_pre_r[identify_armor] = orientation_angle_r + rotate_speed_rps*pre_time;
	//计算瞄中心云台偏转角
	centre_angle_pre_r = atan2(Y_centre_pre, X_centre_pre);
	//计算敌方车辆预测中心距离，单位为mm
	centre_distance_pre_input = X_centre_pre*X_centre_pre + Y_centre_pre*Y_centre_pre;
	arm_sqrt_f32(centre_distance_pre_input, &centre_distance_pre);
	//计算z轴预测补偿高度，线性补偿y=kx，补偿单位为mm
	Z_compensate_pre	= Z_BC * (centre_distance_pre / 1000.0f);
	//计算四块装甲板的数据
	for(uint8_t i = 0; i < 4; i++)
	{
		//分配装甲板半径与预测Z坐标
		if(i % 2 == 0)
		{
			R_armor[i] = radius_1;
			Z_armor_pre[i] = Z_centre_pre + Z_compensate_pre;
		}
		else
		{
			R_armor[i] = radius_2;
			Z_armor_pre[i] = Z_centre_pre + Z_compensate_pre + Z_armor_err;
		}
		//计算四块装甲板的预测朝向角与预测绝对筛选角，单位为rad
		orientation_angle_pre_r[i] = orientation_angle_pre_r[identify_armor] + i*right_angle_r*rotate_direction;
		abs_orientation_angle_pre_r[i] = orientation_angle_pre_r[i] - centre_angle_pre_r;
		//计算四块装甲板的预测坐标，单位为mm
		X_armor_pre[i] = X_centre_pre - R_armor[i]*arm_cos_f32(orientation_angle_pre_r[i]);
		Y_armor_pre[i] = Y_centre_pre - R_armor[i]*arm_sin_f32(orientation_angle_pre_r[i]);
		//计算四块装甲板边界坐标
		X_armor_pre_left[i]  = X_armor_pre[i] - 0.5f * target_armor_width*arm_sin_f32(orientation_angle_pre_r[i]);
		Y_armor_pre_left[i]  = Y_armor_pre[i] + 0.5f * target_armor_width*arm_cos_f32(orientation_angle_pre_r[i]);
		X_armor_pre_right[i] = X_armor_pre[i] + 0.5f * target_armor_width*arm_sin_f32(orientation_angle_pre_r[i]);
		Y_armor_pre_right[i] = Y_armor_pre[i] - 0.5f * target_armor_width*arm_cos_f32(orientation_angle_pre_r[i]);
		//
		yaw_armor_pre_left[i] = atan2(Y_armor_pre_left[i], X_armor_pre_left[i]);
		yaw_armor_pre_right[i] = atan2(Y_armor_pre_right[i], X_armor_pre_right[i]);
	}
	
	// watch_111 = Y_armor_pre_left[0];
	// watch_222 = Y_armor_pre[0];
	// watch_333 = Y_armor_pre_right[0];
	// watch_444 = atan2(Y_armor_pre_left[0], X_armor_pre_left[0]);
	// watch_555 = atan2(Y_armor_pre[0], X_armor_pre[0]);
	// watch_666 = atan2(Y_armor_pre_right[0], X_armor_pre_right[0]);

	switch(rotate_direction)
	{
		case AntiClockwise:
		{
			if(abs_orientation_angle_pre_r[track_armor] < M_PI/4)
			{
				track_status = 1;
				
				if(yaw_armor_pre_left[track_armor] > centre_angle_pre_r)
				{

				}
				else
				{
					track_armor++;
					
					if(track_armor > 3)
						track_armor = 3;
					

				}
			}
			else
			{
				track_status = 2;
			}
		}break;
		
		case Clockwise:
		{
			if(abs_orientation_angle_pre_r[track_armor] > -M_PI/4)
			{
				track_status = 1;
				
				if(yaw_armor_pre_right[track_armor] < centre_angle_pre_r)
				{

				}
				else
				{
					track_armor++;
					
					if(track_armor > 3)
						track_armor = 3;
				}
			}
			else
			{
				track_status = 2;
			}
		}break;
		
		default: break;
	}
	//计算跟踪装甲板yaw轴期望值，弧度单位
	yaw_target_armor = centre_angle_pre_r;
	//计算目标装甲板对应的距离，单位为mm
	target_distance_input = X_armor_pre[track_armor]*X_armor_pre[track_armor] + Y_armor_pre[track_armor]*Y_armor_pre[track_armor];
	arm_sqrt_f32(target_distance_input, &target_distance);
	//目标装甲板长度变量赋值，单位：m
	target_armor_aim.xy0 	= target_distance / 1000.0f;
	target_armor_aim.z0		= Z_armor_pre[track_armor] / 1000.0f;
	//计算跟踪装甲板pitch轴期望值，弧度单位
	projectile_solve(shoot_speed, &target_armor_aim, &ballistic_solusion);
	pitch_target_armor = INFANTRY*ballistic_solusion.ang_solution1;
	//计算装甲板上下边界坐标
	Z_target_armor_up		 = Z_armor_pre[track_armor] + 0.5f * target_armor_height*armor_slant_angle;
	Z_target_armor_down	 = Z_armor_pre[track_armor] - 0.5f * target_armor_height*armor_slant_angle;
	//计算俯仰角边界角度
	target_armor_up_aim.xy0 	= target_distance / 1000.0f;
	target_armor_up_aim.z0		= Z_target_armor_up / 1000.0f;
	target_armor_down_aim.xy0 = target_distance / 1000.0f;
	target_armor_down_aim.z0	= Z_target_armor_down / 1000.0f;
	projectile_solve(shoot_speed, &target_armor_up_aim, &ballistic_solusion_up);
	projectile_solve(shoot_speed, &target_armor_down_aim, &ballistic_solusion_down);
	//计算目标装甲板火控角度
	pitch_target_armor_up 	= INFANTRY*ballistic_solusion_up.ang_solution1;
	pitch_target_armor_down = INFANTRY*ballistic_solusion_down.ang_solution1;
	//计算允许开火角度范围
	pitch_allow_fire_angle_min_r = pitch_target_armor - pitch_target_armor_down;
	pitch_allow_fire_angle_max_r = pitch_target_armor - pitch_target_armor_up;
	//计算跟踪装甲板期望值，弧度单位
	target_set.yaw_targetangle_gyro = yaw_target_armor + INS.Yaw.angle_round*2*M_PI;
	target_set.pitch_targetangle_gyro = pitch_target_armor;
	//控制yaw轴期望值范围
	while(target_set.yaw_targetangle_gyro - INS.Yaw.total_radangle > M_PI)target_set.yaw_targetangle_gyro -= 2*M_PI;
	while(target_set.yaw_targetangle_gyro - INS.Yaw.total_radangle < -M_PI)target_set.yaw_targetangle_gyro += 2*M_PI;


	fire_status = aimcentre_fire_control(yaw_armor_pre_left[track_armor], yaw_armor_pre_right[track_armor], pitch_allow_fire_angle_min_r, pitch_allow_fire_angle_max_r);
	//视觉识别正常
	return Vision_OK;
}
uint8_t Woody_Antioutpost(uint8_t shoot_speed)
{
	//转速量纲为rad/s
	float rotate_speed_rps;
	float yaw_motor_res_speed_rps;
	//时间量纲为s
	float pre_time;
	float bullet_fly_time;
	float shoot_delay_time;
	float vision_delay_time;
	float yaw_motor_res_time;
	//角度量纲为rad
	float advanced_angle_r;
	float yaw_angle_diff_r;
	float centre_angle_pre_r;
	float orientation_angle_r;
	//PC原始数据
	float outpost_radius;
	float X_speed,	Y_speed,Z_speed;
	float X_centre, Y_centre, Z_centre;
	//后处理坐标数据，量纲为mm
	float X_armor, Y_armor, Z_armor;
	float Z_compensate, Z_compensate_pre;
	float X_centre_pre, Y_centre_pre, Z_centre_pre;
	//所有装甲板数据，量纲为mm
	float R_armor[3];
	float orientation_angle_pre_r[3];
	float abs_orientation_angle_pre_r[3];
	float X_armor_pre[3], Y_armor_pre[3], Z_armor_pre[3];
	//需要开根号计算的长度变量，量纲为mm
	float target_distance, target_distance_input;
	float centre_distance, centre_distance_input;
	float centre_distance_pre, centre_distance_pre_input;
	float bullet_fly_distance, bullet_fly_distance_input;
	//目标装甲板参数
	float target_armor_width;
	float target_armor_height;
	//目标装甲板边界坐标
	float Z_target_armor_up, Z_target_armor_down;
	float X_target_armor_left, Y_target_armor_left;
	float X_target_armor_right, Y_target_armor_right;
	//火控允许开火角度范围
	float yaw_allow_fire_angle_min_r, yaw_allow_fire_angle_max_r;
	float pitch_allow_fire_angle_min_r, pitch_allow_fire_angle_max_r;
	//云台火控瞄准角度
	float yaw_target_armor, yaw_target_armor_left, yaw_target_armor_right;
	float pitch_target_armor, pitch_target_armor_up, pitch_target_armor_down;
	//目标装甲板相关变量

	//弹道解算存储结构体
	target_spac_t  target_armor_aim;
	target_spac_t  target_armor_up_aim;
	target_spac_t  target_armor_down_aim;
	//弹道解算反解pitch轴结构体
	ballistic_sol_t ballistic_solusion; 
	ballistic_sol_t ballistic_solusion_up;
	ballistic_sol_t ballistic_solusion_down;
	//判断视觉是否丢失数据
	if(minipc_rx.state_flag == 0)
	{
		target_set.yaw_targetangle_gyro		= INS.Yaw.total_radangle;
		target_set.pitch_targetangle_gyro	= INS.Pitch.total_radangle;
		return Vision_Error;
	}
	//读取PC数据
	X_speed 						= minipc_rx.X_speed;
	Y_speed 						= minipc_rx.Y_speed;
	Z_speed 						= minipc_rx.Z_speed;
	X_centre 						= minipc_rx.X_coordinate;
	Y_centre 						= minipc_rx.Y_coordinate;
	Z_centre 						= minipc_rx.Z_coordinate;
	outpost_radius			= 276.5f;
	orientation_angle_r		= minipc_rx.Yaw_coordinate;
	rotate_speed_rps 		= -1*rotate_direction*0.8f*M_PI;
	vision_delay_time 		= minipc_rx.process_time / 100000.0f;
	//判断装甲板尺寸
	target_armor_width  = small_armor_width;
	target_armor_height = small_armor_height;
	//装甲板识别更新
	armor_update();
	//调试电机旋转速度
	yaw_motor_res_speed_rps = 1.2f;
	//计算敌方车辆中心距离，单位为mm
	centre_distance_input = X_centre*X_centre + Y_centre*Y_centre;
	arm_sqrt_f32(centre_distance_input, &centre_distance);
	//计算z轴补偿高度，线性补偿y=kx，补偿单位为mm
	Z_compensate	= Z_Outpost_BC * (centre_distance / 1000.0f);
	//计算识别装甲板坐标
	X_armor = X_centre - outpost_radius*arm_cos_f32(orientation_angle_r);
	Y_armor = Y_centre - outpost_radius*arm_sin_f32(orientation_angle_r);
	Z_armor = Z_centre + Z_compensate;
	//计算弹丸飞行距离
	bullet_fly_distance_input = X_armor*X_armor + Y_armor*Y_armor + Z_armor*Z_armor;
	arm_sqrt_f32(bullet_fly_distance_input, &bullet_fly_distance);
	//计算弹丸飞行时间
	bullet_fly_time = (bullet_fly_distance / 1000.0f) / shoot_speed;
	//调试发射延时
	shoot_delay_time = 0.0f;
	//计算预测时间
	pre_time = bullet_fly_time + shoot_delay_time + vision_delay_time;
	//预测位置信息
	X_centre_pre = X_centre + X_speed*pre_time;
	Y_centre_pre = Y_centre + Y_speed*pre_time;
	Z_centre_pre = Z_centre + Z_speed*pre_time;
	orientation_angle_pre_r[identify_armor] = orientation_angle_r + rotate_speed_rps*pre_time;
	//计算瞄中心云台偏转角
	centre_angle_pre_r = atan2(Y_centre_pre, X_centre_pre);
	//计算敌方车辆预测中心距离，单位为mm
	centre_distance_pre_input = X_centre_pre*X_centre_pre + Y_centre_pre*Y_centre_pre;
	arm_sqrt_f32(centre_distance_pre_input, &centre_distance_pre);
	//计算z轴预测补偿高度，线性补偿y=kx，补偿单位为mm
	Z_compensate_pre	= Z_Outpost_BC * (centre_distance_pre / 1000.0f);
	//计算四块装甲板的数据
	for(uint8_t i = 0; i < 3; i++)
	{
		//分配装甲板半径与预测Z坐标
		R_armor[i] = outpost_radius;
		//计算四块装甲板的预测朝向角与预测绝对筛选角，单位为rad
		orientation_angle_pre_r[i] = orientation_angle_pre_r[identify_armor] + i*(11*M_PI/18)*rotate_direction;
		abs_orientation_angle_pre_r[i] = orientation_angle_pre_r[i] - centre_angle_pre_r;
		//计算四块装甲板的预测坐标，单位为mm
		X_armor_pre[i] = X_centre_pre - R_armor[i]*arm_cos_f32(orientation_angle_pre_r[i]);
		Y_armor_pre[i] = Y_centre_pre - R_armor[i]*arm_sin_f32(orientation_angle_pre_r[i]);
		Z_armor_pre[i] = Z_centre_pre + Z_compensate_pre;
	}
	
	switch(rotate_direction)
	{
		case AntiClockwise:
		{
			if(abs_orientation_angle_pre_r[track_armor] < M_PI/3)
			{
				track_status = 1;
				
				yaw_angle_diff_r 		= atan2(Y_armor_pre[track_armor], X_armor_pre[track_armor]) - atan2(Y_armor_pre[track_armor + 1], X_armor_pre[track_armor + 1]);
				yaw_motor_res_time 	= fabsf(yaw_angle_diff_r) / yaw_motor_res_speed_rps;
				advanced_angle_r 		= fabsf(rotate_speed_rps) * yaw_motor_res_time / 2.0f;
								
				if(abs_orientation_angle_pre_r[track_armor] < (M_PI/3 - advanced_angle_r))
				{

				}
				else
				{
					track_armor++;
					
					if(track_armor > 2)
						track_armor = 2;

				}
			}
			else
			{
				track_status = 2;
			}
		}break;
		
		case Clockwise:
		{
			if(abs_orientation_angle_pre_r[track_armor] > -M_PI/3)
			{
				track_status = 1;
				
				yaw_angle_diff_r 		= atan2(Y_armor_pre[track_armor], X_armor_pre[track_armor]) - atan2(Y_armor_pre[track_armor + 1], X_armor_pre[track_armor + 1]);
				yaw_motor_res_time 	= fabsf(yaw_angle_diff_r) / yaw_motor_res_speed_rps;
				advanced_angle_r 		= fabsf(rotate_speed_rps) * yaw_motor_res_time / 2.0f;				
				
				if(abs_orientation_angle_pre_r[track_armor] > (-M_PI/3 + advanced_angle_r))
				{

				}
				else
				{
					track_armor++;
					
					if(track_armor > 2)
						track_armor = 2;

				}
			}
			else
			{
				track_status = 2;
			}
		}break;
		
		default: break;
	}
	
	
	//计算跟踪装甲板yaw轴期望值，弧度单位
	yaw_target_armor = atan2(Y_armor_pre[track_armor], X_armor_pre[track_armor]);
	//计算目标装甲板对应的距离，单位为mm
	target_distance_input = X_armor_pre[track_armor]*X_armor_pre[track_armor] + Y_armor_pre[track_armor]*Y_armor_pre[track_armor];
	arm_sqrt_f32(target_distance_input, &target_distance);
	//目标装甲板长度变量赋值，单位：m
	target_armor_aim.xy0 	= target_distance / 1000.0f;
	target_armor_aim.z0		= Z_armor_pre[track_armor] / 1000.0f;
	//计算跟踪装甲板pitch轴期望值，弧度单位
	projectile_solve(shoot_speed, &target_armor_aim, &ballistic_solusion);
	pitch_target_armor = INFANTRY*ballistic_solusion.ang_solution1;
	//计算装甲板边界坐标
	Z_target_armor_up	= Z_armor_pre[track_armor]  + 0.5f * target_armor_height*arm_cos_f32(armor_slant_angle);
	Z_target_armor_down	 = Z_armor_pre[track_armor]  - 0.5f * target_armor_height*arm_cos_f32(armor_slant_angle);
	X_target_armor_left  = X_armor_pre[track_armor]  - 0.5f * target_armor_width*arm_sin_f32(orientation_angle_pre_r[track_armor]);
	Y_target_armor_left  = Y_armor_pre[track_armor]  + 0.5f * target_armor_width*arm_cos_f32(orientation_angle_pre_r[track_armor]);
	X_target_armor_right = X_armor_pre[track_armor]  + 0.5f * target_armor_width*arm_sin_f32(orientation_angle_pre_r[track_armor]);
	Y_target_armor_right = Y_armor_pre[track_armor]  - 0.5f * target_armor_width*arm_cos_f32(orientation_angle_pre_r[track_armor]);
	//计算俯仰角边界角度
	target_armor_up_aim.xy0 	= target_distance / 1000.0f;
	target_armor_up_aim.z0		= Z_target_armor_up / 1000.0f;
	target_armor_down_aim.xy0 = target_distance / 1000.0f;
	target_armor_down_aim.z0	= Z_target_armor_down / 1000.0f;
	projectile_solve(shoot_speed, &target_armor_up_aim, &ballistic_solusion_up);
	projectile_solve(shoot_speed, &target_armor_down_aim, &ballistic_solusion_down);
	//计算目标装甲板火控角度
	yaw_target_armor_left 	= atan2(Y_target_armor_left, X_target_armor_left);
	yaw_target_armor_right 	= atan2(Y_target_armor_right, X_target_armor_right);
	pitch_target_armor_up 	= INFANTRY*ballistic_solusion_up.ang_solution1;
	pitch_target_armor_down = INFANTRY*ballistic_solusion_down.ang_solution1;
	//计算允许开火角度范围
	yaw_allow_fire_angle_min_r = yaw_target_armor - yaw_target_armor_left;
	yaw_allow_fire_angle_max_r = yaw_target_armor - yaw_target_armor_right;
	pitch_allow_fire_angle_min_r = pitch_target_armor - pitch_target_armor_down;
	pitch_allow_fire_angle_max_r = pitch_target_armor - pitch_target_armor_up;
	//计算跟踪装甲板期望值，弧度单位
	target_set.yaw_targetangle_gyro = yaw_target_armor + INS.Yaw.angle_round*2*M_PI;
	target_set.pitch_targetangle_gyro = pitch_target_armor;
	//控制yaw轴期望值范围
	while(target_set.yaw_targetangle_gyro - INS.Yaw.total_radangle > M_PI)target_set.yaw_targetangle_gyro -= 2*M_PI;
	while(target_set.yaw_targetangle_gyro - INS.Yaw.total_radangle < -M_PI)target_set.yaw_targetangle_gyro += 2*M_PI;
	//判断发射状态
	fire_status = autoaim_fire_control(yaw_allow_fire_angle_min_r, yaw_allow_fire_angle_max_r, pitch_allow_fire_angle_min_r, pitch_allow_fire_angle_max_r);
	//视觉识别正常
	return Vision_OK;
}





void projectile_solve(float v0, target_spac_t* target, ballistic_sol_t* solution)
{
	float x0x0 = target->xy0*target->xy0;
	float distance2 = x0x0+target->z0*target->z0;
	float distance = sqrtf(distance2);

	if(v0 == 0.0f || (target->xy0 == 0.0f && target->z0 == 0.0f)){
		solution->solution_num = 0;
		return;
	}
	
	float a = target->z0 + (GRAVITY_G*x0x0/(v0*v0));
	if(a > distance) {
		solution->solution_num = 0;
		return;
	}
	float alpha = asinf(a / distance);
	float phi = M_PI/2;
	if(target->xy0 != 0.0f) phi = atanf(target->z0/target->xy0);

	solution->solution_num = 2;
	solution->ang_solution1 = (alpha+phi)/2;
	solution->ang_solution2 = (M_PI-alpha+phi)/2;
	return;
}

void armor_update(void)
{
	static uint8_t antitop_exit_cnt;
	static uint8_t antitop_update_flag, update_finish_flag;
	static uint8_t antitop_zero_flag, antitop_one_flag;
	static uint8_t antitop_clear_flag, antitop_clear_cnt;
	
	if(armour_update_flag == 0)
	{
		antitop_zero_flag = 1;
		antitop_clear_flag = 1;
	}
	else if(armour_update_flag == 1)
	{
		antitop_one_flag = 1;
		antitop_clear_flag = 2;
	}
	
	if((antitop_zero_flag * antitop_one_flag) == 1)
	{
		antitop_exit_cnt++;
		antitop_update_flag = 1;
	}
	else
	{
		antitop_exit_cnt = 0;
		antitop_update_flag = 0;
	}
	
	if(antitop_clear_flag!=0)
		antitop_clear_cnt++;
	
	if(antitop_exit_cnt >= 10)
		antitop_clear_cnt = 100;
	
	if((antitop_clear_flag == 1)&&(antitop_clear_cnt >= 100))
	{
		antitop_clear_cnt = 0;
		antitop_clear_flag = 0;
		antitop_one_flag = 0;
	}
	else if((antitop_clear_flag == 2)&&(antitop_clear_cnt >= 100))
	{
		antitop_clear_cnt = 0;
		antitop_clear_flag = 0;
		antitop_zero_flag = 0;
	}

	if((antitop_update_flag == 1)&&(update_finish_flag == 0))
	{
		if(wait_armor > 0)
			wait_armor--;
		else
			wait_armor = 0;
		if(track_armor > 0)
			track_armor--;
		else
			track_armor = 0;
		
		update_finish_flag = 1;
	}
	else if((antitop_update_flag == 1)&&(update_finish_flag == 1))
	{
		update_finish_flag = 1;
	}
	else if((antitop_update_flag == 0)&&(update_finish_flag == 1))
	{
		update_finish_flag = 0;
	}
	else if((antitop_update_flag == 0)&&(update_finish_flag == 0))
	{
		update_finish_flag = 0;
	}
}

uint8_t top_status_judge(float rotate_speed)
{
	static uint8_t start_cord_flag;
	static float time_cord ,top_time;
	
	if(fabs(rotate_speed) < 0.6)	
	{
		start_cord_flag=0;
		return 0;
	}
	if(fabs(rotate_speed) >= 0.6 && start_cord_flag == 0)
	{
		time_cord=DWT_GetTimeline_s();
		start_cord_flag=1;
	}
	if(start_cord_flag == 1 && fabs(rotate_speed) >= 0.6f)
	{
		top_time = DWT_GetTimeline_s() - time_cord;
	}
	if(top_time >0.6f)
	{
		return 1;
	}
	return 0;
}

uint8_t aimcenter_judge(float rotate_speed,float aim_speed)
{
	static uint8_t start_cord_flag;
	static float time_cord;

	if(fabsf(rotate_speed) <=aim_speed)	
	{
		start_cord_flag=0;
		return 0;
	}	
	if(fabsf(rotate_speed)>aim_speed && start_cord_flag == 0)
	{
		time_cord=DWT_GetTimeline_s();
		start_cord_flag=1;		
	}
	if(start_cord_flag == 1 && (DWT_GetTimeline_s() - time_cord )>= 0.45)
	{
    return 1;
	}
	else
		return 0;
}

int8_t top_direction_judge(float rotate_speed)
{
	int8_t top_dir;
	static uint8_t start_cord_flag;
	static float time_cord ,top_time;
	
	if(fabs(rotate_speed) < 0.6)	
	{
		start_cord_flag=0;
		return 0;
	}
	if(fabs(rotate_speed) >= 0.6&& start_cord_flag == 0)
	{
		time_cord=DWT_GetTimeline_s();
		start_cord_flag=1;
	}
	if(start_cord_flag==1 && fabs(rotate_speed) >= 0.6f )
	{
		top_time = DWT_GetTimeline_s() - time_cord;
	}
	if(top_time > 0.6f)
	{
		if(rotate_speed > 0.0f)
			top_dir = AntiClockwise;
		else if(rotate_speed < 0.0f)
			top_dir = Clockwise;
		else
			top_dir = 0;
		return top_dir;
	}
	return 0;
}

uint8_t autoaim_fire_control(float yaw_fire_angle_min, float yaw_fire_angle_max, float pitch_fire_angle_min, float pitch_fire_angle_max)
{
	uint8_t fire_status;
	
	static float yaw_angle_err_r;
	static float pitch_angle_err_r;
		
	yaw_angle_err_r = target_set.yaw_targetangle_gyro - INS.Yaw.total_radangle;
	pitch_angle_err_r = target_set.pitch_targetangle_gyro - INS.Pitch.total_radangle;
	
	if(yaw_angle_err_r < yaw_fire_angle_max &&  yaw_angle_err_r > yaw_fire_angle_min && 		 
	   pitch_angle_err_r < pitch_fire_angle_max && pitch_angle_err_r > pitch_fire_angle_min)
	{
		fire_status = Fire_OK;
	}
	else
	{
		fire_status = Fire_Err;
	}
	
	return fire_status;
}

uint8_t aimcentre_fire_control(float yaw_armor_left, float yaw_armor_right, float pitch_fire_angle_min, float pitch_fire_angle_max)
{
	uint8_t fire_status;
	
	float pitch_angle_err_r;
		
	pitch_angle_err_r = target_set.yaw_targetangle_gyro - INS.Yaw.total_radangle;

	if(INS.Yaw.total_radangle < yaw_armor_left  && \
		 INS.Yaw.total_radangle > yaw_armor_right && \
		 pitch_angle_err_r < pitch_fire_angle_max && \
		 pitch_angle_err_r > pitch_fire_angle_min)
	{
		fire_status = Fire_OK;
	}
	else
	{
		fire_status = Fire_Err;
	}
	
	return fire_status;
}